#include <stdio.h> 
#include <stdlib.h> 
#include <memory.h>
#include <string.h>

#include <ctime>

#include <dos.h>

#include <stdio.h>
#include <windows.h>
#include <math.h>
#include <string>

#if defined(_WIN32) || defined(WIN32) || defined(__WIN32__)
#include <conio.h>
#endif

#include <iostream>
#include <fstream>
#include <sstream>


#include "inertia.h"
#include "serial.h" 
//#include "joystick.h"

#define NOP 0x00
#define ANGLES 0x11
#define SET_POINTS 0x12
#define JOYSTICK 0x13
#define SETPWM 0x24
#define KPARAMS 0x35

#define SIZE_DATOS 11

time_t tiempo;
typedef struct {
        int roll;
        int pitch;
        int yaw;
}sensor;

void GotoXY(int x, int y);
void ClearScreen();

void Serial_Port_sub();
void Keyboard_sub();
void Joystick_sub();
void Inertia_sub();
void Screen_sub();

using namespace std;

int roll=0;
ISD_TRACKER_HANDLE      handle;
ISD_TRACKER_INFO_TYPE   tracker;
ISD_TRACKING_DATA_TYPE  data;
char letter;
int grados;
//Joystick joy;
//JoystickStatus stat;
//JoystickStatus stat_old;
string debug;
bool power_on = false;
sensor inertia,inertia_old;
char kp;
char kd;
char ki;
float kptof(char kp);
char mot1, mot2;

char archivo[16];
char datos[SIZE_DATOS];
bool save_data = false;


int curr_file = 0;

static fstream ofs;
void log_data(bool );

int main()
{

    if(!rs_initialise("\\\\.\\COM5" ,57600, '8', 'N')) { getch(); exit(1); };

    printf("InertiaCube Connection? (y/n) \n");

    while (!kbhit());

    if (getch()=='y'){
        handle = ISD_OpenTracker( NULL, 0, FALSE, FALSE ); 
        
    }else{
          printf ( "WARNING: Inertiacube deactivated\n");
    }
 
    while (!kbhit());

    debug = "";
    power_on = false;
    kp = 0;
    kd = 0;
    ki = 0;

    ClearScreen();
  
    while( 1)
    {
         Serial_Port_sub();
         Keyboard_sub();
     //    Joystick_sub();
         Inertia_sub();
         Screen_sub();

       Sleep( 6 );
    }

    ISD_CloseTracker( handle );

}

//*******************************************************************
//                         SERIAL PORT                                                               
//*******************************************************************
void Serial_Port_sub(){

int i;

        if (char r=rs_getch()){
           switch (r){
                  case 0x01:   //Start of Heading (debug string)
                       debug = "";
                       while ((r=rs_getch())!=0x04)                             
                             debug += r;
                       ClearScreen();
                       break;

                  case 'D':

                       for (i=0;i<SIZE_DATOS;i++){
                          r=rs_getch();
                          datos[i]=r;
                          printf("%i,",datos[i]);
                       }

                       if (save_data)
                          
                          log_data(false);
                          

                       break;
           }
       }

}

//*******************************************************************
//                           SCREEN                                                               
//*******************************************************************
void Screen_sub(){
      GotoXY(0,0);
      printf("Joystick Input:\n");
//      printf("X: %d     \nY: %d         \nZ: %d         \n",stat.x*100/32767,stat.y*100/32767,stat.z*100/32767);

      printf("%s\n\n", power_on?"POWER ON ":"POWER OFF" );

//      printf("Mot1: %d     \nMot2: %d         \nZ: %d         \n",((stat.x*100/32767)/10)+100,-((stat.x*100/32767)/10)+100,stat.z*100/32767);
      printf("Mot1: %i     \nMot2: %i         \n",mot1,mot2);

//      printf("SetPoint: %d   \n" ,((stat.x*100/32767)/1)); 
      
      printf("\nKP:%f  \n",kptof(kp));
      printf("\nKD:%f  \n",kptof(kd));
      printf("\nKI:%f  \n",kptof(ki));

      printf("\nInertiaCube:\n");
      printf("Rl: %d     \nAz: %d         \nEl: %d\n",inertia.roll,0,inertia.pitch);


      printf("\nDebug: %s ",debug.c_str());

}

//*******************************************************************
//                           Keyboard                                                               
//*******************************************************************

void Keyboard_sub(){

   if( kbhit() )
    {
        switch (getch()){
             case 'q':
                  exit(0);
                  break;

             case 'c':
                  debug = "" ;
                  ClearScreen();
                  break;

             case '1':                      
                  rs_putch(KPARAMS);  
                  rs_putch('k'); 
             break;

             case '2':                      
             break;
             
             case 'a':
             case 'A':
                     save_data = true;
                     debug = "Recording...";
                     ofs.open("log.csv", ios::out);
                  break;

             case 's':                      
             case 'S':                     
                     char output[100];

                     save_data = false;
                     debug = "                  ";
                     ofs.close();
                     sprintf(output,"rename.bat %d",time(&tiempo));                      
                     system("wgnuplot plot_save.cfg");
                     system(output);        
          
//                     system("wgnuplot plot.cfg");
             break;

             case 'p':                      
             case 'P':                      
                  kp++;
                  rs_putch(KPARAMS);
                  rs_putch(kp);
                  rs_putch(kd);
                  rs_putch(ki);
             break;

             case 'r':
             case 'R':
                rs_putch(JOYSTICK);
                rs_putch(0x01);
                power_on = true;
                break;

             case 'T':
             case 't':
                rs_putch(JOYSTICK);
                rs_putch(0x00);
                power_on = false;
                break;


             case 'o':                      
             case 'O': 
                  if (kp>0) kp--;                     
                  rs_putch(KPARAMS);
                  rs_putch(kp);
                  rs_putch(kd);
                  rs_putch(ki);
                  break;

             case 'l':                      
             case 'L':                      
                  kd++;
                  rs_putch(KPARAMS);
                  rs_putch(kp);
                  rs_putch(kd);
                  rs_putch(ki);
             break;

             case 'k':                      
             case 'K': 
                  if (kd>0) kd--;                     
                  rs_putch(KPARAMS);
                  rs_putch(kp);
                  rs_putch(kd);
                  rs_putch(ki);
             break;

             case 'm':                      
             case 'M':                      
                  ki++;
                  rs_putch(KPARAMS);
                  rs_putch(kp);
                  rs_putch(kd);
                  rs_putch(ki);
             break;

             case 'n':                      
             case 'N': 
                  if (ki>0) ki--;                     
                  rs_putch(KPARAMS);
                  rs_putch(kp);
                  rs_putch(kd);
                  rs_putch(ki);
             break;

             case 'v':                      
             case 'V': 
                  rs_putch(SET_POINTS);
                  rs_putch(20);
             break;

             case 'b':                      
             case 'B': 
                  rs_putch(SET_POINTS);
                  rs_putch(0);
             break;
             

        }
    }
}

//*******************************************************************
//                           InertiaCube                                                              
//*******************************************************************

void Inertia_sub(){
        if(handle > 0)
        {
           ISD_GetTrackingData( handle,    &data );
                             
           if ( (data.Station[0].Euler[2]>-100) && (data.Station[0].Euler[2]<100) ){
             inertia.pitch = (int)ceil(data.Station[0].Euler[1]);
             inertia.roll = (int)ceil(data.Station[0].Euler[2]);


             if(inertia.roll!=inertia_old.roll){
                rs_putch(ANGLES);  //Angles 1 byte
                rs_putch(inertia.roll); 
             }

             inertia_old = inertia;
           }

     
         }else{
         //   exit(0);
         }
}

//*******************************************************************
//                           Joystick                                                              
//*******************************************************************

void Joystick_sub(){
//     joy.GetStatus(stat);
/*
     if (stat.button1 == 1){
        while (stat.button1==1)
             joy.GetStatus(stat);

        rs_putch(JOYSTICK);
        rs_putch(0x01);
        power_on = true;
     }

     if (stat.button3 == 1){
        while (stat.button3==1)
             joy.GetStatus(stat);

        rs_putch(JOYSTICK);
        rs_putch(0x00);
        power_on=false;

        if (power_on==false) {
           power_on = true;            
        }else{
           power_on = false;
            rs_putch(SETPWM);
            rs_putch(47);
            rs_putch(47);

        }

     }

     if ((power_on==true) && (stat.x!=stat_old.x)){

//            rs_putch(SET_POINTS);
//            rs_putch(((stat.x*100/32767)/1));

     //     rs_putch(SETPWM);
      //    rs_putch(((stat.x*100/32767)/5)+65);
       //   rs_putch(-((stat.x*100/32767)/5)+65);
      }

      stat_old = stat;
      */
}



float kptof(char kp){
      float out=0;
      float div = 1;
      for(int i=7;i>=0;i--)
      {
       out+=div*((kp&(1<<i))?1:0);
       div=div/2;
      }

      return out;
/*
      if ((kp&128)==128) 
         out = 1;
      else
          out = 0;
      
      for (int i=1;i<6;i++)
          if (kp&(2^i)!=0)
               out += 2^(-i);

      return out;
*/
}

void log_data(bool new_file)
{
//   if(new_file){curr_file++;}
//       sprintf (archivo, "log.csv");
//       ofs.open(archivo, ios::out);

       for(int i=0; i<SIZE_DATOS-1;i++)
       {
               ofs << (int)datos[i] << ',';

       }
       ofs << (int)datos[SIZE_DATOS-1]<< endl;
  
}

void GotoXY(int x, int y)
{
  static HANDLE hStdOut = GetStdHandle(STD_OUTPUT_HANDLE);
  static COORD WritePos;
  WritePos.X = x; //-1 for borland compatibility??
  WritePos.Y = y;
  SetConsoleCursorPosition(hStdOut,WritePos);
}

void ClearScreen(){
  HANDLE hndl = GetStdHandle(STD_OUTPUT_HANDLE);
  CONSOLE_SCREEN_BUFFER_INFO csbi;
  GetConsoleScreenBufferInfo(hndl, &csbi);
  DWORD written;
  DWORD n = csbi.dwSize.X * csbi.dwCursorPosition.Y + csbi.dwCursorPosition.X + 1;	
  COORD curhome = {0,0};
  FillConsoleOutputCharacter(hndl, ' ', n, curhome, &written);
  csbi.srWindow.Bottom -= csbi.srWindow.Top;
  csbi.srWindow.Top = 0;
  SetConsoleWindowInfo(hndl, TRUE, &csbi.srWindow);
  SetConsoleCursorPosition(hndl, curhome);
}

